/********************************************************************************

 **** Copyright (C), 2018, Fortior Technology Co., Ltd.                      ****

 ********************************************************************************
 * File Name     : AddFunction.c
 * Author        : Bruce HW&RD
 * Date          : 2018-12-27
 * Description   : .C file function description
 * Version       : 1.0
 * Function List :
 *
 * Record        :
 * 1.Date        : 2018-12-27
 *   Author      : Bruce HW&RD
 *   Modification: Created file

********************************************************************************/

#include "FU68xx_4.h"
#include <MyProject.h>

/* 全局参数 */
FaultStateType mcFaultSource;
FaultVarible idata mcFaultDect;
CurrentVarible idata mcCurVarible;
ProtectVarible idata mcProtectTime;
LOWSPEEDCONCTROL LowSpeedCtrl;
FOCCTRL xdata mcFocCtrl;
ADCSample xdata AdcSampleValue;
MCRAMP xdata mcSpeedRamp;
MotorFRTypeDef xdata mcFRState;

uint16 Break_delay_time = 0;

/* 私有函数 */
static int16 HW_One_PI(int16 Xn1);
static void mc_ramp(MCRAMP *hSpeedramp);

/*---------------------------------------------------------------------------*/
/* Name     :   void FaultProcess(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 保护处理函数，关闭FOC输出，同时将状态变为mcFault
/*---------------------------------------------------------------------------*/
void FaultProcess(void)
{
    ClrBit(DRV_CR, DRVEN); // Driver Disable
    ClrBit(DRV_CR, FOCEN); // FOC Disable
    MOE = 0;
    mcState = mcFault;
}

/*****************************************************************************
 * Function:         void   Fault_OverVoltage(mcFaultVarible *h_Fault)
 * Description:  过压欠压保护函数：
 *               1.程序每5ms判断一次
 *               2.母线电压大于过压保护值时，计数器加一
 *               3.计数器值超过20次，判断为过压保护，关闭输出;
 *               4.反之，计数器慢慢减
                 5.同理，欠压保护。
                 6.电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，
                 7.过压恢复值以下时，计数器加一，超过200次后，恢复
                 8.根据档位信息来决定恢复到哪个状态。
 * Parameter:        mcFaultVarible *h_Fault
 * Return:           no
 *****************************************************************************/
static void Fault_OverUnderVoltage(FaultVarible *h_Fault)
{
    /* 故障标志 */
    uint8 flag_fault = 0;

    /* 无其他故障 */
    if (mcFaultSource == FaultNoSource)
    {
        /* 过压保护 */
        /* 母线电压大于过压保护值时，计数，超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减 */
        if (mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)
        {
            if (++h_Fault->OverVoltDetecCnt > 10) // 检测100ms
            {
                h_Fault->OverVoltDetecCnt = 0;
                mcFaultSource = FaultOverVoltage;
                flag_fault = 1;
            }
        }
        /* 欠压保护 */
        else if(mcFocCtrl.mcDcbusFlt < UNDER_PROTECT_VALUE)
        {
            ResetBUZZERPin;
            IRScan.IRReceiveFlag = 0;
            ClrBit(TIM4_CR1, T4IF);

            if (++h_Fault->OverVoltDetecCnt > 10) // 检测100ms
            {
                h_Fault->OverVoltDetecCnt = 0;
                mcFaultSource = FaultUnderVoltage;
                flag_fault = 1;
            }
        }
        /* 正常范围 */
        else
        {
            if (h_Fault->OverVoltDetecCnt > 0)
            {
                h_Fault->OverVoltDetecCnt--;
            }
        }
        /* 有故障 */
        if(flag_fault == 1)
        {
            FaultProcess();
        }
    }

    /* 过压欠压保护恢复 */
    if ((mcState == mcFault) && ((mcFaultSource == FaultOverVoltage) || (mcFaultSource == FaultUnderVoltage)))
    {
        if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE))
        {
            h_Fault->VoltRecoverCnt++;
            if (h_Fault->VoltRecoverCnt > 200) // 连续检测1s，若正常则恢复
            {
                mcState = mcReady;
                mcFaultSource = FaultNoSource;
                h_Fault->VoltRecoverCnt = 0;
            }
        }
        else
        {
            h_Fault->VoltRecoverCnt = 0;
        }
    }
}

/*****************************************************************************
 * Function:     void Fault_Overcurrent(CurrentVarible *h_Cur)
 * Description:  电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则OverCurCnt加1。
                 连续累加3次，判断为软件过流保护。执行时间约30.4us。
 * Parameter:    mcFaultVarible *h_Fault
 * Return:       no
 *****************************************************************************/
void Fault_Overcurrent(CurrentVarible *h_Cur)
{
    if ((mcState == mcRun) || (mcState == mcStart)) // check over current in rum and open mode
    {
        h_Cur->Max_ia = FOC__IAMAX;
        h_Cur->Max_ib = FOC__IBMAX;
        h_Cur->Max_ic = FOC__ICMAX;
        mcCurVarible.Max_ia = FOC__IAMAX;
        mcCurVarible.Max_ib = FOC__IBMAX;
        mcCurVarible.Max_ic = FOC__ICMAX;

        if ((FOC__IAMAX >= OverSoftCurrentValue) || \
            (FOC__IBMAX >= OverSoftCurrentValue) || \
            (FOC__ICMAX >= OverSoftCurrentValue))
        {
            if (++h_Cur->OverCurCnt >= 3)
            {
                FaultProcess();
                mcFaultSource = FaultSoftOVCurrent;
                h_Cur->Max_ia = 0;
                h_Cur->Max_ib = 0;
                h_Cur->Max_ic = 0;
                h_Cur->OverCurCnt = 0;
            }
        }
        else if (h_Cur->OverCurCnt > 0)
        {
            h_Cur->OverCurCnt--;
        }
    }
}

/*****************************************************************************
 * Function:         void   Fault_OverCurrentRecover(mcFaultVarible *h_Fault)
 * Description:      软硬件过流保护恢复
 * Parameter:        mcFaultVarible *h_Fault
 * Return:           no
 *****************************************************************************/
static void Fault_OverCurrentRecover(FaultVarible *h_Fault)
{
    if ((mcState == mcFault) && \
       ((mcFaultSource == FaultSoftOVCurrent) || (mcFaultSource == FaultHardOVCurrent)) && \
        (mcProtectTime.CurrentPretectTimes < 5))
    {
        if (++h_Fault->CurrentRecoverCnt >= OverCurrentRecoverTime) // 200*5=1s
        {
            h_Fault->CurrentRecoverCnt = 0;
            mcProtectTime.CurrentPretectTimes++;
            mcState = mcReady;
            mcFaultSource = FaultNoSource;
        }
    }
}

/*****************************************************************************
 * Function:         void   Fault_Start(mcFaultVarible *h_Fault)
 * Description:   启动保护函数，
 *                电机运行状态下，电机在前5s估算转速达到堵转保护值或者5s后反电动势值太低(此方法未验证)
                  或4s内还在CtrlMode状态，
                  即速度低于MOTOR_LOOP_RPM，
                  程序判断为启动失败，
                  电机停机。
                  当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进入校准状态，等待重启。
 * Parameter:        mcFaultVarible *h_Fault
 * Return:           no
 *****************************************************************************/
static void Fault_Start(FaultVarible *h_Fault)
{
    /*******启动保护恢复*********/
    if (mcState == mcRun)
    {
        // 方法一，5s内速度大于最大速度，同时反电动势值低于一定值
        if (h_Fault->StartSpeedCnt <= 1000)
        {
            h_Fault->StartSpeedCnt++;
            if ((mcFocCtrl.SpeedFlt > Motor_Max_Speed) && (mcFocCtrl.EsValue < 1800))
            {
                h_Fault->StartSpeedCnt = 0;
                mcFaultSource = FaultStart;
                FaultProcess();
                mcProtectTime.SecondStartTimes++;
                mcProtectTime.StartFlag = 1;
            }
        }
    }
    #if (!StartONOFF_Enable)
        if ((mcFaultSource == FaultStart) && (mcState == mcFault) && \
            (mcProtectTime.SecondStartTimes <= StartProtectRestartTimes))
        {
            mcFaultSource = FaultNoSource;
            mcState = mcInit;
        }
    #endif
}

/*****************************************************************************
* Function:         void   Fault_Stall(mcFaultVarible *h_Fault)
* Description:  堵转保护函数，有三种保护方式，
                  第一种，
                  第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速度连续5次；
                  第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值连续6次；
                  当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护；
                  当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于或等于堵转重启次数8次，
                  程序延迟mcStallRecover重新启动，进行校准状态。
* Parameter:        mcFaultVarible *h_Fault
* Return:           no
*****************************************************************************/
static void Fault_Stall(FaultVarible *h_Fault)
{
    if ((mcState == mcRun) && (Time.mcRunStateCount > 5000))
    {
        // method 1，判断反电动势太小或当反电动势太小，转速太大
        if ((mcFocCtrl.EsValue < 20) || \
            (mcFocCtrl.EsValue > 0xFFF0) || \
            ((FOC__EOME < _Q15(65.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 2000)))
        {
            if (++h_Fault->StallDectEs >= 5)
            {
                h_Fault->StallDectEs = 0;
                mcFaultSource = FaultStall;
                mcProtectTime.StallTimes++;
                mcProtectTime.StallFlag = 1;
                FaultProcess();
            }
        }
        else
        {
            if (h_Fault->StallDectEs > 0)
            {
                h_Fault->StallDectEs--;
            }
        }

        // method 2，判断速度低于堵转最小值或者超过堵转最大值
        if (((mcFocCtrl.SpeedFlt < _Q15(100.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 1800)) || (mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
        {
            if (++h_Fault->StallDectSpeed >= 5)
            {
                h_Fault->StallDectSpeed = 0;
                mcFaultSource = FaultStall;
                mcProtectTime.StallTimes++;
                mcProtectTime.StallFlag = 2;
                FaultProcess();
            }
        }
        else
        {
            if (h_Fault->StallDectSpeed > 0)
            {
                h_Fault->StallDectSpeed--;
            }
        }
    }

#if (!StartONOFF_Enable)

    /*******堵转保护恢复*********/
    if ((mcFaultSource == FaultStall) && (mcState == mcFault) && (mcProtectTime.StallTimes <= StallProtectRestartTimes))
    {
        h_Fault->StallReCount++;

        if (h_Fault->StallReCount >= StallRecoverTime)
        {
            h_Fault->StallReCount = 16000;
            mcFaultSource = FaultNoSource;
            mcState = mcInit;
        }
    }
    else
    {
        h_Fault->StallReCount = 0;
    }

#endif
}

/*
    缺相保护
*/
static void Fault_phaseloss(FaultVarible *h_Fault)
{
    uint8 flag_fault = 0;

    if (mcState == mcRun)
    {
        if (++h_Fault->Lphasecnt > 200) // 100*5=500ms
        {
            h_Fault->Lphasecnt = 0;

            if (((mcCurVarible.Max_ia > (mcCurVarible.Max_ib * 2)) || (mcCurVarible.Max_ia > (mcCurVarible.Max_ic * 2))) 
			&& (mcCurVarible.Max_ia > PhaseLossCurrentValue))
            {
                h_Fault->AOpencnt++;
            }
            else
            {
                if (h_Fault->AOpencnt > 0)
                {
                    h_Fault->AOpencnt--;
                }
            }

            if (((mcCurVarible.Max_ib > (mcCurVarible.Max_ia * 2)) || (mcCurVarible.Max_ib > (mcCurVarible.Max_ic * 2))) 
			&& (mcCurVarible.Max_ib > PhaseLossCurrentValue))
            {
                h_Fault->BOpencnt++;
            }
            else
            {
                if (h_Fault->BOpencnt > 0)
                {
                    h_Fault->BOpencnt--;
                }
            }

            if (((mcCurVarible.Max_ic > (mcCurVarible.Max_ia * 2)) || (mcCurVarible.Max_ic > (mcCurVarible.Max_ib * 2))) 
			&& (mcCurVarible.Max_ic > PhaseLossCurrentValue))
            {
                h_Fault->COpencnt++;
            }
            else
            {
                if (h_Fault->COpencnt > 0)
                {
                    h_Fault->COpencnt--;
                }
            }

            mcCurVarible.Max_ia = 0;
            mcCurVarible.Max_ib = 0;
            mcCurVarible.Max_ic = 0;

            if ((h_Fault->AOpencnt > 1) || (h_Fault->BOpencnt > 1) || (h_Fault->COpencnt > 1))
            {
                mcProtectTime.LossPHTimes++;
                mcFaultSource = FaultLossPhase;
                flag_fault = 1;
            }
        }

        if (((mcCurVarible.Max_ia < 200) && (mcCurVarible.Max_ib < 200) && (mcCurVarible.Max_ic < 200)) && ((mcFocCtrl.Powerlpf < 400) || (mcFocCtrl.Powerlpf > 60000)) && ((Time.mcRunStateCount > 2000)))
        {
            if (h_Fault->Lpha3secnt++ > 100) //
            {
                h_Fault->Lpha3secnt = 0;
                mcProtectTime.LossPHTimes++;
                mcFaultSource = FaultLossPhase;
                flag_fault = 1;
            }
        }
        else
        {
            h_Fault->Lpha3secnt = 0;
        }

        if(flag_fault == 1)
        {
            FaultProcess();
        }
    }

#if (!StartONOFF_Enable)

    /*******缺相保护恢复*********/
    if ((mcFaultSource == FaultLossPhase) && (mcState == mcFault) && (mcProtectTime.LossPHTimes <= 5)) // 可重启5次
    {
        if (++h_Fault->mcLossPHRecCount >= PhaseLossRecoverTime)
        {
            h_Fault->AOpencnt = 0;
            h_Fault->BOpencnt = 0;
            h_Fault->COpencnt = 0;
            mcState = mcReady;
            mcFaultSource = FaultNoSource;
        }
    }
    else
    {
        h_Fault->mcLossPHRecCount = 0;
    }

#endif
}

/*---------------------------------------------------------------------------*/
/* Name     :   void Fault_Detection(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时器中断执行一次对应的保护
                  常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根据需求，一个个的调试加入。
/*---------------------------------------------------------------------------*/
void Fault_Detection(void)
{
    if (++mcFaultDect.segment >= 5)
    {
        mcFaultDect.segment = 0;
    }
    
    switch (mcFaultDect.segment)
    {
    case 0:
        //if (CurrentRecoverEnable) // 过流保护恢复使能
        {
            Fault_OverCurrentRecover(&mcFaultDect);
        }
        break;
    case 1:
        //if (VoltageProtectEnable == 1) // 过压保护使能
        {
            Fault_OverUnderVoltage(&mcFaultDect);
        }
        break;
    case 2:
        //if (StartProtectEnable == 1) // 启动保护使能
        {
            Fault_Start(&mcFaultDect);
        }
        break;
    case 3:
        //if (StallProtectEnable == 1) // 堵转保护使能
        {
            Fault_Stall(&mcFaultDect);
        }
        break;
    case 4:
        //if (PhaseLossProtectEnable == 1) // 缺相保护使能
        {
            Fault_phaseloss(&mcFaultDect);
        }
        break;
    default:
        break;
    }
}

/*---------------------------------------------------------------------------*/
/* Name     :   void Speed_response(void)
/* Input    :   NO
/* Output   :   NO
/* Description: 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制、恒功率控制
/*---------------------------------------------------------------------------*/
void Speed_response(void)
{
    /* 遥控更新转速 */
    if (IRControl.FlagSpeed)
    {
        mcSpeedRamp.TargetValue = IRControl.SpeedLevel[IRControl.TargetSpeed];
        IRControl.FlagSpeed = 0;
    }

    /* 运行或者停止状态下 */
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        switch (mcFocCtrl.CtrlMode)
        {
        case 0:
        {
            if (mcFocCtrl.SpeedFlt > Motor_Loop_Speed) //>=MOTOR_LOOP_RPM
            {
                mcFocCtrl.CtrlMode = 1;
                FOC_DQKP = DQKP;
                FOC_DQKI = DQKI;
                FOC_IDREF = 0;
                PI_Init();
#if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                {
                    mcSpeedRamp.ActualValue = MOTOR_SPEED_LEVEL_2;
                }
#elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                {
                    mcSpeedRamp.ActualValue = mcFocCtrl.Powerlpf;
                }
#endif
                mcFocCtrl.OUTLoopCtrl.PIControlPeriod = SPEED_LOOP_TIME;
                PI3_UKH = mcFocCtrl.mcIqref;
                mcFocCtrl.CtrlMode = 1;
                FOC__THECOMP = _Q15(0.0 / 180.0);
                FOC_QMAX = FOC__UQ;
                FOC_QMIN = QOUTMIN;
            }
            else
            {
                mcFocCtrl.State_Count = 1680;
                FOC_EKP = OBSW_KP_GAIN; // 估算器里的PI的KP
                FOC_EKI = OBSW_KI_GAIN; // 估算器里的PI的KI
                if (FOC_IQREF < IQ_RUN_CURRENT)
                {
                    mcFocCtrl.mcIqref += QOUTINC;

                    if (mcFocCtrl.mcIqref > IQ_RUN_CURRENT)
                    {
                        mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
                    }

                    FOC_IQREF = mcFocCtrl.mcIqref;
                }
                else if ((FOC_IQREF > IQ_RUN_CURRENT))
                {
                    mcFocCtrl.mcIqref -= QOUTINC;

                    if (mcFocCtrl.mcIqref < IQ_RUN_CURRENT)
                    {
                        mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
                    }

                    FOC_IQREF = mcFocCtrl.mcIqref;
                }
            }
        }
        break;

        case 1:
        {
            if (++mcFocCtrl.OUTLoopCtrl.TorqueLoopTime > mcFocCtrl.OUTLoopCtrl.PIControlPeriod)
            {
                mcFocCtrl.OUTLoopCtrl.TorqueLoopTime = 0;
#if (OUTLoop_Mode == OUTLoop_Disable)
                {
                    mcFocCtrl.mcIqref = FOC_IQREF;

                    if (FOC_IQREF < QOUTMAXVALUE)
                    {
                        mcFocCtrl.mcIqref += QOUTINC;

                        if (mcFocCtrl.mcIqref > QOUTMAXVALUE)
                        {
                            mcFocCtrl.mcIqref = QOUTMAXVALUE;
                        }

                        FOC_IQREF = mcFocCtrl.mcIqref;
                    }
                    else if (FOC_IQREF > QOUTMAXVALUE)
                    {
                        mcFocCtrl.mcIqref -= QOUTINC;

                        if (mcFocCtrl.mcIqref < QOUTMAXVALUE)
                        {
                            mcFocCtrl.mcIqref = QOUTMAXVALUE;
                        }

                        FOC_IQREF = mcFocCtrl.mcIqref;
                    }
                }
#elif (OUTLoop_Mode == OUTLoop_Enable)
                {
                    if (FOC_QMAX < QOUTMAX)
                    {
                        FOC_QMAX += 200; // 修复FOC_QMIN变化是Iq调节异常
                    }

                    mc_ramp(&mcSpeedRamp);

                    if (mcSpeedRamp.FlagONOFF)
                    {
#if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                        {
                            mcFocCtrl.mcIqref = HW_One_PI(mcSpeedRamp.ActualValue - mcFocCtrl.SpeedFlt); // COMPESATE是作为补偿
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
#elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {

                            mcFocCtrl.mcIqref = HW_One_PI(mcSpeedRamp.ActualValue - mcFocCtrl.Powerlpf);
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
#endif
                    }
                    else
                    {
                        mcFocCtrl.mcIqref = HW_One_PI(-mcFocCtrl.SpeedFlt);
                        FOC_IQREF = mcFocCtrl.mcIqref;
                    }
                }
#endif // END OUTLoop_Mode

                if (FOC_EK2 < OBS_K2T_Actual - 10)
                {
                    mcFocCtrl.Smo_EK2 = FOC_EK2;
                    mcFocCtrl.Smo_EK2 += 10;
                    FOC_EK2 = mcFocCtrl.Smo_EK2;
                }
                else if (FOC_EK2 > OBS_K2T_Actual + 10)
                {
                    mcFocCtrl.Smo_EK2 = FOC_EK2;
                    mcFocCtrl.Smo_EK2 -= 10;
                    FOC_EK2 = mcFocCtrl.Smo_EK2;
                }
                else
                {
                    FOC_EK2 = OBS_K2T_Actual;
                }
            }
        }
        break;
        }
    }
}

/*---------------------------------------------------------------------------*/
/* Name     :   void mc_ramp(void)
/* Input    :   hTarget,MC_RAMP *hSpeedramp
/* Output   :   NO
/* Description:
/*---------------------------------------------------------------------------*/
static void mc_ramp(MCRAMP *hSpeedramp)
{
    if (--hSpeedramp->DelayCount < 0)
    {
        hSpeedramp->DelayCount = hSpeedramp->DelayPeriod;

        if (hSpeedramp->ActualValue < hSpeedramp->TargetValue)
        {
            if (hSpeedramp->ActualValue + Motor_Speed_Inc < hSpeedramp->TargetValue)
            {
                hSpeedramp->ActualValue += Motor_Speed_Inc;
            }
            else
            {
                hSpeedramp->ActualValue = hSpeedramp->TargetValue;
            }
        }
        else
        {
            if (hSpeedramp->ActualValue - Motor_Speed_Dec > hSpeedramp->TargetValue)
            {
                hSpeedramp->ActualValue -= Motor_Speed_Dec;
            }
            else
            {
                hSpeedramp->ActualValue = hSpeedramp->TargetValue;
            }
        }
    }
}

/*-------------------------------------------------------------------------------------------------
    Function Name : int16 HW_One_PI(int16 Xn1, int16 Yn0, int16 Xn2)
    Description   : PI控制
    Input         : Xn1--E(K)
    Output        : PI_UK--当前PI输出值,执行时间us
-------------------------------------------------------------------------------------------------*/
static int16 HW_One_PI(int16 Xn1)
{
    PI3_EK = Xn1;          // 填入EK
    SetBit(PI_CR, PI3STA); // Start PI
    while (ReadBit(PI_CR, PIBSY))
        ;
    return PI3_UKH;
}

/*---------------------------------------------------------------------------*/
/* Name     :   void StarRampDealwith(void)
/* Input    :   NO
/* Output   :   NO
/* Description:
/*---------------------------------------------------------------------------*/
void StarRampDealwith(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))
    {
        switch (mcFocCtrl.State_Count)
        {
        case 1600:
            FOC_EKP = OBSW_KP_GAIN_RUN; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN; // 估算器里的PI的KI
            break;
        case 1200:
            FOC_EKP = OBSW_KP_GAIN_RUN1; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1; // 估算器里的PI的KI
            break;
        case 1000:
            FOC_EKP = OBSW_KP_GAIN_RUN2; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2; // 估算器里的PI的KI
            break;
        case 800:
            FOC_EKP = OBSW_KP_GAIN_RUN3; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3; // 估算器里的PI的KI
            break;
        case 400:
            FOC_EKP = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
            break;
        case 200:
            FOC_EKP = OBSW_KP_GAIN_RUN5; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN5; // 估算器里的PI的KI
            break;
        default:
            break;
        }
    #if 0
        if (mcFocCtrl.State_Count == 1600) // 2300
        {
            FOC_EKP = OBSW_KP_GAIN_RUN; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 1200) // 2000
        {
            FOC_EKP = OBSW_KP_GAIN_RUN1; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 1000) // 1600
        {
            FOC_EKP = OBSW_KP_GAIN_RUN2; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 800) // 1200
        {
            FOC_EKP = OBSW_KP_GAIN_RUN3; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 400) // 1200
        {
            FOC_EKP = OBSW_KP_GAIN_RUN4; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4; // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 200)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN5; // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN5; // 估算器里的PI的KI
        }
    #endif
    }
}

void CurrentZeroDetect(void)
{
    static uint16 CurrentDetectTime = 0;

    if (TailWindDetect.TailWindBrakeStatus)
    {
        MOE = 0;
        FOC_CR1 = 0x00;
        ClrBit(DRV_CR, FOCEN);
        DRV_DR = DRV_ARR + 1;
        DRV_CMR &= 0xFFC0;
        DRV_CMR |= 0x015;    // 三相下桥臂通，刹车
        ClrBit(DRV_CR, OCS); // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
        MOE = 1;
        mcFocCtrl.State_Count = TailWindDetect.BrakeRelaxTime;

        if (++CurrentDetectTime < Break_delay_time)
        {
            ;
        }
        else
        {
            TailWindDetect.TailWindBrakeStatus = 0;
            TailWindDetect.BrakeCurrentDetectStatus = 1;
        }
    }
    else
    {
        CurrentDetectTime = 0;
    }
}

void Fault_Buzzer(void)
{
    if ((mcProtectTime.over_VoltageTimes > 5) ||
        (mcProtectTime.under_VoltageTimes > 5) ||
        (mcProtectTime.StallTimes > 5) ||
        (mcProtectTime.LossPHTimes > 5))
    {
        if (Time.BuzzTimer % 2000 == 0) // 1500
        {
            SetBuzzer(1, 2, 1);
        }
    }
    else
    {
        Time.BuzzTimer = 0;
    }

#if 0
    if (mcProtectTime.over_VoltageTimes > 5)
    {
        if (Time.BuzzTimer % 100 == 0) // 1500
        {
            SetBuzzer(1, 2, 2);
        }
    }
    else if (mcProtectTime.under_VoltageTimes > 5)
    {
        if (Time.BuzzTimer % 600 == 0) // 1500
        {
            SetBuzzer(1, 2, 2);
        }
    }
    else if (mcProtectTime.StallTimes > 5)
    {
        if (Time.BuzzTimer % 1000 == 0) // 1500
        {
            SetBuzzer(1, 2, 5);
        }
    }
    else if (mcProtectTime.LossPHTimes > 5)
    {
        if (Time.BuzzTimer % 2000 == 0) // 1500
        {
            SetBuzzer(1, 2, 1);
        }
    }
    else
    {
        Time.BuzzTimer = 0;
    }
#endif
}
